#include "slam_ros/publish_occupancy_grid_map.h"
#include <opencv2/opencv.hpp>
#include "slam/common/port.h"
#include "slam/io/file_writer.h"
#include "slam/io/image.h"
#include "slam/io/internal/mapping_state_serialization.h"


namespace slam_ros {

PublishOccupancyGridMap::PublishOccupancyGridMap(std::shared_ptr<rclcpp::Node> node,
    const double resolution, const int map_save_time_interval)
    : node_(node),
      resolution_(resolution),
      map_save_time_interval_(map_save_time_interval),
      map_server_2d_(map_frame_id_) {
  
  submap_query_client_ = node_->create_client<slam_ros_msgs::srv::SubmapQuery> (
      kSubmapQueryServiceName);
  
  submap_list_subscriber_ = node_->create_subscription<slam_ros_msgs::msg::SubmapList>(
      kSubmapListTopic, rclcpp::SystemDefaultsQoS(),
      [this](const slam_ros_msgs::msg::SubmapList::SharedPtr msg) {
        HandleSubmapList(msg);
      });
  
  query_slamstatus_client_ = node_->create_client<slam_ros_msgs::srv::QuerySlamStatus>(
      slam_ros::kQuerySlamStatus);
  
  get_occupancy_map_service_ = node_->create_service<nav_msgs::srv::GetMap>(
      slam_ros::kGetOccupancyGrid,
      [this](const std::shared_ptr<rmw_request_id_t> request_header,
             const std::shared_ptr<nav_msgs::srv::GetMap::Request> request,
             const std::shared_ptr<nav_msgs::srv::GetMap::Response> response) {
        HandleGetOccupancyGrid(request_header, request, response);
      });
  
  get_map_data_service_ = node_->create_service<slam_ros_msgs::srv::GetMapData>(
      slam_ros::kGetMapData,
      [this](const std::shared_ptr<rmw_request_id_t> request_header,
             const std::shared_ptr<slam_ros_msgs::srv::GetMapData::Request> request,
             const std::shared_ptr<slam_ros_msgs::srv::GetMapData::Response> response) {
        HandleGetMapData(request_header, request, response);
      });
  
  get_map_point_service_ = node_->create_service<agv_srvs::srv::GetMapPoint>(
      slam_ros::kGetMapPng,
      [this](const std::shared_ptr<rmw_request_id_t> request_header,
             const std::shared_ptr<agv_srvs::srv::GetMapPoint::Request> request,
             const std::shared_ptr<agv_srvs::srv::GetMapPoint::Response> response) {
        HandleGetMapPoint(request_header, request, response);
      });
  
  save_map_timer_ = node_->create_wall_timer(
      std::chrono::seconds(map_save_time_interval_),
      [this]() { SaveMappingMap(); });
  
  map_publisher_ = node_->create_publisher<agv_msgs::msg::MapPng>("map_data", 2);

}

void PublishOccupancyGridMap::HandleGetMapPoint(
    const std::shared_ptr<rmw_request_id_t> request_header,
    const std::shared_ptr<agv_srvs::srv::GetMapPoint::Request> request,
    const std::shared_ptr<agv_srvs::srv::GetMapPoint::Response> response) {
  (void)request_header;
  
  if (!request->status) {
    return;
  }
  
  if (!query_slamstatus_client_->wait_for_service(std::chrono::seconds(1))) {
    RCLCPP_ERROR(node_->get_logger(), "Service not available");
    return;
  }

  RCLCPP_INFO(node_->get_logger(), "Call HandleGetMapPoint");

  auto query_slamstatus_srv = std::make_shared<slam_ros_msgs::srv::QuerySlamStatus::Request>();
  query_slamstatus_srv->queryslam = 1;
  
  auto response_callback = [this, response](rclcpp::Client<slam_ros_msgs::srv::QuerySlamStatus>::SharedFuture future) {
    auto res = future.get();
    if (!res) {
      RCLCPP_ERROR(node_->get_logger(), "Service returned null response");
      return;
    }

    std::string slam_status;
    slam_type_ = static_cast<SlamType>(res->slamstatus);
    switch (slam_type_) {
      case SlamType::UNKNOWN:
        slam_status = "Slam status: UNKNOWN";
        break;
      case SlamType::LOCALIZATION:
        slam_status = "Slam status: LOCALIZATION";
        break;
      case SlamType::MAPPING:
        slam_status = "Slam status: MAPPING";
        break;
      default:
        break;
    }
   
    if (slam_type_ == SlamType::MAPPING) {
      return;
    } else if (slam_type_ == SlamType::LOCALIZATION) {
      auto occupancy_map = std::make_shared<nav_msgs::msg::OccupancyGrid>();
      auto map_response = std::make_shared<nav_msgs::srv::GetMap::Response>();
      const std::string yaml_file = res->mapfile + ".yaml";
      
      RCLCPP_INFO(node_->get_logger(), "cur yaml_file is %s", yaml_file.c_str());
      if (access(yaml_file.c_str(), 0) != -1) {
        map_server_2d_.loadMapFromYaml(map_response.get(), yaml_file);
        *occupancy_map = map_response->map;
      } else {
        RCLCPP_ERROR(node_->get_logger(), "The png file: %s does not exist!", yaml_file.c_str());
        return;
      }
      
      const std::string png_file = res->mapfile + ".png";
      if (access(png_file.c_str(), 0) != -1) {
        cv::Mat cv_image = cv::imread(png_file);
        std::vector<uchar> vecImg;
        std::vector<int> vecCompression_params;
        vecCompression_params.push_back(cv::IMWRITE_JPEG_QUALITY);
        vecCompression_params.push_back(90);
        cv::imencode(".png", cv_image, vecImg, vecCompression_params);
        std::string img_data = base64Encode(vecImg.data(), vecImg.size());
        
        response->resolution = occupancy_map->info.resolution;
        response->data = img_data;
        response->height = occupancy_map->info.height;
        response->width = occupancy_map->info.width;
        response->origin.position.x = occupancy_map->info.origin.position.x;
        response->origin.position.y = occupancy_map->info.origin.position.y;
      } else {
        RCLCPP_ERROR(node_->get_logger(), "The png file: %s does not exist!", png_file.c_str());
        return;
      }
    }
  };

  query_slamstatus_client_->async_send_request(query_slamstatus_srv, response_callback);
}

void PublishOccupancyGridMap::HandleGetMapData(
    const std::shared_ptr<rmw_request_id_t> request_header,
    const std::shared_ptr<slam_ros_msgs::srv::GetMapData::Request> request,
    const std::shared_ptr<slam_ros_msgs::srv::GetMapData::Response> response) {
  (void)request_header;
  
  if (!request->status) {
    return;
  }

  auto occupancy_map = std::make_shared<nav_msgs::srv::GetMap::Response>();
  GetMapResponse(occupancy_map);

  response->resolution = occupancy_map->map.info.resolution;
  response->width = occupancy_map->map.info.width;
  response->height = occupancy_map->map.info.height;
  response->origin = occupancy_map->map.info.origin;

  auto& map_data = occupancy_map->map.data;
  for (auto iter = map_data.begin(); iter != map_data.end(); ++iter) {
    const char pixel =
        *iter == -1 ? 128 : (255 * slam::common::RoundToInt(1 - *iter / 100.0));
    response->data.emplace_back(pixel);
  }
}

void PublishOccupancyGridMap::HandleGetOccupancyGrid(
    const std::shared_ptr<rmw_request_id_t> request_header,
    const std::shared_ptr<nav_msgs::srv::GetMap::Request> request,
    const std::shared_ptr<nav_msgs::srv::GetMap::Response> response) {
  (void)request_header;
  (void)request;
  GetMapResponse(response);
}

void PublishOccupancyGridMap::GetMapResponse(
    std::shared_ptr<nav_msgs::srv::GetMap::Response> map_data_response) {
  if (!query_slamstatus_client_->wait_for_service(std::chrono::seconds(1))) {
    RCLCPP_ERROR(node_->get_logger(), "Service not available");
    return;
  }

  RCLCPP_INFO(node_->get_logger(), "Call GetMapResponse");

  auto query_slamstatus_srv = std::make_shared<slam_ros_msgs::srv::QuerySlamStatus::Request>();
  query_slamstatus_srv->queryslam = 1;
  
  auto response_callback = [this, map_data_response](rclcpp::Client<slam_ros_msgs::srv::QuerySlamStatus>::SharedFuture future) {
    auto response = future.get();
    if (!response) {
      RCLCPP_ERROR(node_->get_logger(), "Service returned null response");
      return;
    }

    std::string slam_status;
    slam_type_ = static_cast<SlamType>(response->slamstatus);
    switch (slam_type_) {
      case SlamType::UNKNOWN:
        slam_status = "Slam status: UNKNOWN";
        break;
      case SlamType::LOCALIZATION:
        slam_status = "Slam status: LOCALIZATION";
        break;
      case SlamType::MAPPING:
        slam_status = "Slam status: MAPPING";
        break;
      default:
        break;
    }
    RCLCPP_INFO(node_->get_logger(), "%s", slam_status.c_str());
 
    if (slam_type_ == SlamType::MAPPING) {
      GetMappingMap(map_data_response);
    } else if (slam_type_ == SlamType::LOCALIZATION) {
      const std::string yaml_file = response->mapfile + ".yaml";
      if (access(yaml_file.c_str(), 0) != -1) {
        map_server_2d_.loadMapFromYaml(map_data_response.get(), yaml_file);
      } else {
        RCLCPP_ERROR(node_->get_logger(), "The yaml file: %s does not exist!", yaml_file.c_str());
        return;
      }
    }
  };

  query_slamstatus_client_->async_send_request(query_slamstatus_srv, response_callback);
}

void PublishOccupancyGridMap::HandleSubmapList(
    const slam_ros_msgs::msg::SubmapList::SharedPtr msg) {
  std::lock_guard<std::mutex> lock(mutex_);
  if (slam_type_ == SlamType::LOCALIZATION) {
    return;
  }

  // Keep track of submap IDs that don't appear in the message anymore.
  std::set<SubmapId> submap_ids_to_delete;
  for (const auto& pair : submap_slices_) {
    submap_ids_to_delete.insert(pair.first);
  }

  for (const auto& submap_msg : msg->submap) {
    const SubmapId id {submap_msg.trajectory_id, submap_msg.submap_index};
    submap_ids_to_delete.erase(id);
    if ((submap_msg.is_frozen && !include_frozen_submaps_) ||
        (!submap_msg.is_frozen && !include_unfrozen_submaps_)) {
      continue;
    }
    SubmapSlice& submap_slice = submap_slices_[id];
    submap_slice.pose = ToRigid3d(submap_msg.pose);
    submap_slice.metadata_version = submap_msg.submap_version;
    if (submap_slice.surface != nullptr &&
        submap_slice.version == submap_msg.submap_version) {
      continue;
    }

    while (!submap_query_client_->wait_for_service(std::chrono::seconds(1))) {
      if (!rclcpp::ok()) {
        RCLCPP_ERROR(node_->get_logger(), "Interrupted while waiting for the service. Exiting.");
        return;
      }
      RCLCPP_INFO(node_->get_logger(), "service not available, waiting again...");
    }

    auto srv_request = std::make_shared<slam_ros_msgs::srv::SubmapQuery::Request>();
    srv_request->trajectory_id = id.trajectory_id;
    srv_request->submap_index = id.submap_index;

    using ServiceResponseFuture =
      ::rclcpp::Client<slam_ros_msgs::srv::SubmapQuery>::SharedFuture;
    auto response_received_callback = [&submap_slice](ServiceResponseFuture future) {
      auto fetched_textures = slam_ros::FetchSubmapTextures(future.get());
      if (fetched_textures == nullptr) {
        return;
      }
      CHECK(!fetched_textures->textures.empty());
      submap_slice.version = fetched_textures->version;

      // We use the first texture only. By convention this is the highest
      // resolution texture and that is the one we want to use to construct the
      // map for ROS.
      const auto fetched_texture = fetched_textures->textures.begin();
      submap_slice.width = fetched_texture->width;
      submap_slice.height = fetched_texture->height;
      submap_slice.slice_pose = fetched_texture->slice_pose;
      submap_slice.resolution = fetched_texture->resolution;
      submap_slice.cairo_data.clear();
      submap_slice.surface =  ::slam::io::DrawTexture(
          fetched_texture->pixels.intensity, fetched_texture->pixels.alpha,
          fetched_texture->width, fetched_texture->height,
          &submap_slice.cairo_data);
    };

    auto future_result = submap_query_client_->async_send_request(srv_request, response_received_callback);
  }

  // Delete all submaps that didn't appear in the message.
  for (const auto& id : submap_ids_to_delete) {
    submap_slices_.erase(id);
  }

  last_timestamp_ = msg->header.stamp;
  last_frame_id_ = msg->header.frame_id;
}

void PublishOccupancyGridMap::GetMappingMap(
    std::shared_ptr<nav_msgs::srv::GetMap::Response> map_data_response) {
  std::lock_guard<std::mutex> lock(mutex_);
  if (submap_slices_.empty()) {
    return;
  }
  auto result = PaintSubmapSlices(submap_slices_, resolution_);

  map_data_response->map = *(CreateOccupancyGridMsg(
      result, resolution_, map_frame_id_, last_timestamp_));
}

void PublishOccupancyGridMap::SaveMappingMap() {
  std::lock_guard<std::mutex> lock(mutex_);

  if (!query_slamstatus_client_->wait_for_service(std::chrono::seconds(1))) {
    RCLCPP_ERROR(node_->get_logger(), "Service not available");
    return;
  }

  RCLCPP_INFO(node_->get_logger(), "Call SaveMappingMap");
  auto query_slamstatus_srv = std::make_shared<slam_ros_msgs::srv::QuerySlamStatus::Request>();
  query_slamstatus_srv->queryslam = 1;

  auto response_callback = [this](rclcpp::Client<slam_ros_msgs::srv::QuerySlamStatus>::SharedFuture future) {
    auto response = future.get();
    if (!response) {
      RCLCPP_ERROR(node_->get_logger(), "SlamStatus service returned null response");
      return;
    }

    slam_type_ = static_cast<SlamType>(response->slamstatus);
    if (slam_type_ == SlamType::LOCALIZATION) {
      RCLCPP_INFO(node_->get_logger(), "Localization mode, skipping mapping map save.");
      return;
    }

    if (submap_slices_.empty()) {
      RCLCPP_WARN(node_->get_logger(), "Submap slices are empty, cannot save map.");
      return;
    }
    
    auto result_paint = PaintSubmapSlices(submap_slices_, resolution_);

    slam::io::Image image(std::move(result_paint.surface));
    Eigen::Vector2d origin(-result_paint.origin.x() * resolution_,
                         (result_paint.origin.y() - image.height()) * resolution_);

    image.WriteJpg(kCurrentMappingMap + ".jpg");
    image.WritePng(kCurrentMappingMap + ".png");

    slam::io::StreamFileWriter yaml_writer(kCurrentMappingMap + ".yaml");
    slam::io::WriteYaml(resolution_, origin, kCurrentMappingMap + ".jpg",
                       &yaml_writer);

    std::vector<unsigned char> imge_bytes = image.readPng(kCurrentMappingMap + ".png");

    std::string img_data = base64Encode(imge_bytes.data(), imge_bytes.size());
    auto map_png = std::make_unique<agv_msgs::msg::MapPng>();
    map_png->header.stamp = node_->now();
    map_png->resolution = resolution_;
    map_png->data = img_data;
    map_png->height = image.height();
    map_png->width = image.width();
    map_png->origin.position.x = origin.x();
    map_png->origin.position.y = origin.y();
    map_publisher_->publish(std::move(map_png));
    RCLCPP_INFO(node_->get_logger(), "Successfully saved and published mapping map.");
  };

  query_slamstatus_client_->async_send_request(query_slamstatus_srv, response_callback);
}

std::string PublishOccupancyGridMap::base64Encode(const unsigned char* Data, int DataByte) {
  // Encoding table
  const char EncodeTable[] = "ABCDEFGHIJKLMNOPQRSTUVWXYZabcdefghijklmnopqrstuvwxyz0123456789+/";
  // Return value
  std::string strEncode;
  unsigned char Tmp[4] = {0};
  int LineLength = 0;
  for (int i = 0; i < (int)(DataByte / 3); i++) {
    Tmp[1] = *Data++;
    Tmp[2] = *Data++;
    Tmp[3] = *Data++;
    strEncode += EncodeTable[Tmp[1] >> 2];
    strEncode += EncodeTable[((Tmp[1] << 4) | (Tmp[2] >> 4)) & 0x3F];
    strEncode += EncodeTable[((Tmp[2] << 2) | (Tmp[3] >> 6)) & 0x3F];
    strEncode += EncodeTable[Tmp[3] & 0x3F];
  }
  // Encode remaining data
  int Mod = DataByte % 3;
  if (Mod == 1) {
    Tmp[1] = *Data++;
    strEncode += EncodeTable[(Tmp[1] & 0xFC) >> 2];
    strEncode += EncodeTable[((Tmp[1] & 0x03) << 4)];
    strEncode += "==";
  }
  else if (Mod == 2) {
    Tmp[1] = *Data++;
    Tmp[2] = *Data++;
    strEncode += EncodeTable[(Tmp[1] & 0xFC) >> 2];
    strEncode += EncodeTable[((Tmp[1] & 0x03) << 4) | ((Tmp[2] & 0xF0) >> 4)];
    strEncode += EncodeTable[((Tmp[2] & 0x0F) << 2)];
    strEncode += "=";
  }

  return strEncode;
}

}  // namespace slam_ros